#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Trigger.h>//成员类型 bool string

ros::Publisher turtle_vel_pub;//创建一个发布者对象
bool pubcommand=false;

bool commandCallback(std_srvs::Trigger::Request& req,
					std_srvs::Trigger::Response& res)
{
	pubcommand=!pubcommand;
	
	//打印请求数据
	ROS_INFO("回调函数");
	
	res.success=true;
	res.message="Change turtle command state";
	
	return true;	
}

int main(int argc,char** argv)
{
	//节点初始化
	ros::init(argc,argv,"turtle_command_server");

	ros::NodeHandle n;
	
	//创建一个服务端，注册其回调函数
	ros::ServiceServer command_service = n.advertiseService("/turtle_command",commandCallback);
	//一旦这个服务端相应，就会使用一个线程去调用这个回调函数
	
	//初始化一个发布者，注册一个话题，消息类型为 geometry_msgs::Twis，消息对列长度10，可以存放一个10个对象的队列
	turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
	
	ROS_INFO("Recevice turtle command");
	ros::Rate loop_rate(10);
	
	while(ros::ok())
	{
		ros::spinOnce();
		
		//
		if(pubcommand)
		{
			geometry_msgs::Twist vel_msg;
			vel_msg.linear.x=0.5;
			vel_msg.angular.z=0.2;
			turtle_vel_pub.publish(vel_msg);//使用发布者对象将这个消息对象发布出去，放进发布者队列中
		}
		
		loop_rate.sleep();//按照设定进行延时
	
	}
	return 0;
}


